package com.example.icar.rosmaster;

import android.util.Log;

/**
 * Rosmaster库，用于控制Rosmaster小车
 */
public class RosmasterLib {
    private static final String TAG = "RosmasterLib";
    
    // 小车类型
    public static final int CARTYPE_X1 = 1;
    public static final int CARTYPE_X3 = 3;
    public static final int CARTYPE_X3_PLUS = 4;
    public static final int CARTYPE_X1_PLUS = 5;
    
    // 运动状态
    public static final int MOTOR_STOP = 0;
    public static final int MOTOR_FORWARD = 1;
    public static final int MOTOR_BACKWARD = 2;
    public static final int MOTOR_LEFT = 3;
    public static final int MOTOR_RIGHT = 4;
    
    // 命令功能码
    private static final int FUNC_BTN_CONTROL = 0x15; // 按键控制功能码
    private static final int FUNC_CAR_RUN = 0x11;     // 小车运行功能码
    private static final int FUNC_SERVO = 0x12;       // 舵机控制功能码
    private static final int FUNC_BEEP = 0x13;        // 蜂鸣器控制功能码
    private static final int FUNC_LED = 0x20;         // LED控制功能码
    
    // HTTP命令类型常量
    public static final String HTTP_CMD_MOVE = "move";
    public static final String HTTP_CMD_SERVO = "servo";
    public static final String HTTP_CMD_LED = "led";
    public static final String HTTP_CMD_BEEP = "beep";
    public static final String HTTP_CMD_ARM = "arm";
    public static final String HTTP_CMD_FOLLOW_LINE = "follow_line"; // 寻迹指令命令
    
    private int carType = CARTYPE_X3;
    private boolean debug = false;
    
    /**
     * 构造函数
     */
    public RosmasterLib() {
        this(CARTYPE_X3, false);
    }
    
    /**
     * 构造函数
     * @param carType 小车类型
     * @param debug 是否开启调试模式
     */
    public RosmasterLib(int carType, boolean debug) {
        this.carType = carType;
        this.debug = debug;
        
        if (debug) {
            Log.d(TAG, "RosmasterLib initialized with car type: " + carType);
        }
    }
    
    /**
     * 设置小车运动
     * @param state 运动状态
     * @param speed 速度
     * @return 命令字符串
     */
    public String setCarRun(int state, int speed) {
        if (debug) {
            Log.d(TAG, "setCarRun: state=" + state + ", speed=" + speed);
        }
        
        // 构建多个小车类型的命令，并一起返回，由RosmasterHttpClient依次尝试发送
        StringBuilder commandBuilder = new StringBuilder();
        
        // 尝试类型0 (通用类型)
        int carType0 = 0;
        int checksum0 = (carType0 + FUNC_BTN_CONTROL + 0x04 + state) % 256;
        String command0 = String.format("$%02x%02x%02x%02x%02x#", 
                carType0, FUNC_BTN_CONTROL, 0x04, state, checksum0);
        commandBuilder.append(command0);
        
        if (debug) {
            Log.d(TAG, "生成多类型命令: " + commandBuilder.toString());
        }
        
        // 返回优化的命令格式，只使用类型0，这是通用的
        return command0;
    }
    
    /**
     * 尝试不同小车类型的命令 (用于探测小车类型)
     * @param state 运动状态
     * @param speed 速度
     * @return 所有可能的命令格式，以|分隔
     */
    public String tryAllCarTypes(int state, int speed) {
        StringBuilder commandBuilder = new StringBuilder();
        
        // 尝试类型0 (通用类型)
        int carType0 = 0;
        int checksum0 = (carType0 + FUNC_BTN_CONTROL + 0x04 + state) % 256;
        String command0 = String.format("$%02x%02x%02x%02x%02x#", 
                carType0, FUNC_BTN_CONTROL, 0x04, state, checksum0);
        commandBuilder.append(command0).append("|");
        
        // 尝试X1类型
        int carType1 = CARTYPE_X1;
        int checksum1 = (carType1 + FUNC_BTN_CONTROL + 0x04 + state) % 256;
        String command1 = String.format("$%02x%02x%02x%02x%02x#", 
                carType1, FUNC_BTN_CONTROL, 0x04, state, checksum1);
        commandBuilder.append(command1).append("|");
        
        // 尝试类型2 (可能存在的类型)
        int carType2 = 2;
        int checksum2 = (carType2 + FUNC_BTN_CONTROL + 0x04 + state) % 256;
        String command2 = String.format("$%02x%02x%02x%02x%02x#", 
                carType2, FUNC_BTN_CONTROL, 0x04, state, checksum2);
        commandBuilder.append(command2).append("|");
        
        // 尝试X3类型 (原默认类型)
        int carType3 = CARTYPE_X3;
        int checksum3 = (carType3 + FUNC_BTN_CONTROL + 0x04 + state) % 256;
        String command3 = String.format("$%02x%02x%02x%02x%02x#", 
                carType3, FUNC_BTN_CONTROL, 0x04, state, checksum3);
        commandBuilder.append(command3).append("|");
        
        // 尝试X3_PLUS类型
        int carType4 = CARTYPE_X3_PLUS;
        int checksum4 = (carType4 + FUNC_BTN_CONTROL + 0x04 + state) % 256;
        String command4 = String.format("$%02x%02x%02x%02x%02x#", 
                carType4, FUNC_BTN_CONTROL, 0x04, state, checksum4);
        commandBuilder.append(command4).append("|");
        
        // 尝试X1_PLUS类型
        int carType5 = CARTYPE_X1_PLUS;
        int checksum5 = (carType5 + FUNC_BTN_CONTROL + 0x04 + state) % 256;
        String command5 = String.format("$%02x%02x%02x%02x%02x#", 
                carType5, FUNC_BTN_CONTROL, 0x04, state, checksum5);
        commandBuilder.append(command5);
        
        return commandBuilder.toString();
    }
    
    /**
     * 设置舵机角度
     * @param servoId 舵机ID
     * @param angle 角度
     * @return 命令字符串
     */
    public String setServoAngle(int servoId, int angle) {
        if (debug) {
            Log.d(TAG, "setServoAngle: servoId=" + servoId + ", angle=" + angle);
        }
        
        // 限制角度范围
        if (angle < 0) angle = 0;
        if (angle > 180) angle = 180;
        
        // 构建命令
        int checksum = (carType + FUNC_SERVO + 0x06 + servoId + angle) % 256;
        String command = String.format("$%02x%02x%02x%02x%02x%02x#", 
                carType, FUNC_SERVO, 0x06, servoId, angle, checksum);
                
        return command;
    }
    
    /**
     * 设置LED灯
     * @param ledId LED ID
     * @param r 红色值
     * @param g 绿色值
     * @param b 蓝色值
     * @return 命令字符串
     */
    public String setLed(int ledId, int r, int g, int b) {
        if (debug) {
            Log.d(TAG, "setLed: ledId=" + ledId + ", r=" + r + ", g=" + g + ", b=" + b);
        }
        
        // 限制RGB值范围
        r = Math.min(Math.max(r, 0), 255);
        g = Math.min(Math.max(g, 0), 255);
        b = Math.min(Math.max(b, 0), 255);
        
        // 构建命令
        int checksum = (carType + FUNC_LED + 0x08 + ledId + r + g + b) % 256;
        String command = String.format("$%02x%02x%02x%02x%02x%02x%02x%02x#", 
                carType, FUNC_LED, 0x08, ledId, r, g, b, checksum);
                
        return command;
    }
    
    /**
     * 设置蜂鸣器
     * @param time 时间（毫秒）
     * @return 命令字符串
     */
    public String setBuzzer(int time) {
        if (debug) {
            Log.d(TAG, "setBuzzer: time=" + time);
        }
        
        // 根据小车代码中的解析逻辑转换时间参数
        int state = 0;
        int delay = 0;
        
        if (time > 0) {
            state = 1;
            if (time == 1) {
                delay = 255;  // 特殊值，表示持续蜂鸣
            } else {
                delay = time / 10;
                if (delay > 255) delay = 255;
            }
        }
        
        // 构建命令
        int checksum = (carType + FUNC_BEEP + 0x06 + state + delay) % 256;
        String command = String.format("$%02x%02x%02x%02x%02x%02x#", 
                carType, FUNC_BEEP, 0x06, state, delay, checksum);
                
        return command;
    }
    
    /**
     * 获取小车类型
     * @return 小车类型
     */
    public int getCarType() {
        return carType;
    }
    
    /**
     * 设置小车类型
     * @param carType 小车类型
     */
    public void setCarType(int carType) {
        this.carType = carType;
    }
    
    /**
     * 设置调试模式
     * @param debug 是否开启调试模式
     */
    public void setDebug(boolean debug) {
        this.debug = debug;
    }
    
    /**
     * 生成HTTP命令
     * @param cmdType 命令类型
     * @param params 参数
     * @return HTTP命令字符串
     */
    public String generateHttpCommand(String cmdType, String params) {
        if (debug) {
            Log.d(TAG, "generateHttpCommand: cmdType=" + cmdType + ", params=" + params);
        }
        
        // 构建HTTP命令
        return cmdType + "?" + params;
    }
    
    /**
     * 设置寻迹指令的HTTP命令
     * @param flag 1开启，0关闭
     * @return HTTP命令字符串
     */
    public String setFollowLineHttp(int flag) {
        if (debug) {
            Log.d(TAG, "setFollowLine: flag=" + flag);
        }
        
        // 使用寻迹功能码：0x36
        int carType = 0;  // 使用类型0
        int funcCode = 0x36;  // 寻迹功能码
        int len = 0x06;  // 长度为6
        int data = flag;  // 数据位：1开启，0关闭
        
        // 计算校验和：carType + funcCode + length + data
        int checksum = (carType + funcCode + len + data) % 256;
        
        // 生成命令：$TYPE+FUNC+LEN+DATA+CHECK#
        StringBuilder command = new StringBuilder();
        command.append("$");
        command.append(String.format("%02x", carType));    // 00
        command.append(String.format("%02x", funcCode));   // 36
        command.append(String.format("%02x", len));        // 06
        command.append(String.format("%02x", data));       // 00或01
        command.append(String.format("%02x", checksum));   // 校验和
        command.append("#");
        
        String tcpCommand = command.toString();
        
        if (debug) {
            Log.d(TAG, String.format("生成寻迹命令: %s, 小车类型=0x%02x, 功能码=0x%02x, 长度=0x%02x, 数据=0x%02x, 校验和=0x%02x", 
                    tcpCommand, carType, funcCode, len, data, checksum));
        }
        
        return tcpCommand;
    }

    /**
     * 设置小车运动的HTTP命令
     * @param state 运动状态
     * @param speed 速度
     * @return HTTP命令字符串
     */
    public String setCarRunHttp(int state, int speed) {
        if (debug) {
            Log.d(TAG, "setCarRunHttp: state=" + state + ", speed=" + speed);
        }
        
        // 尝试直接生成TCP命令格式，这样更可能被小车识别
        int checksum = (0 + 0x15 + 0x04 + state) % 256; // 使用类型0，避免car_type错误
        String tcpCommand = String.format("$%02x%02x%02x%02x%02x#", 
                0, 0x15, 0x04, state, checksum);
        
        if (debug) {
            Log.d(TAG, "生成运动TCP命令: " + tcpCommand);
        }
        
        return tcpCommand;
    }
    
    /**
     * 设置舵机角度（HTTP格式）
     * @param servoId 舵机ID
     * @param angle 角度
     * @return HTTP命令字符串
     */
    public String setServoAngleHttp(int servoId, int angle) {
        if (debug) {
            Log.d(TAG, "setServoAngleHttp: servoId=" + servoId + ", angle=" + angle);
        }
        
        // 限制角度范围
        if (angle < 0) angle = 0;
        if (angle > 180) angle = 180;
        
        return generateHttpCommand(HTTP_CMD_SERVO, "id=" + servoId + "&angle=" + angle);
    }
    
    /**
     * 设置LED灯（HTTP格式）
     * @param ledId LED ID
     * @param r 红色值
     * @param g 绿色值
     * @param b 蓝色值
     * @return HTTP命令字符串
     */
    public String setLedColorHttp(int ledId, int r, int g, int b) {
        if (debug) {
            Log.d(TAG, "setLedColorHttp: ledId=" + ledId + ", r=" + r + ", g=" + g + ", b=" + b);
        }
        
        // 限制颜色范围
        r = Math.min(255, Math.max(0, r));
        g = Math.min(255, Math.max(0, g));
        b = Math.min(255, Math.max(0, b));
        
        return generateHttpCommand(HTTP_CMD_LED, "id=" + ledId + "&r=" + r + "&g=" + g + "&b=" + b);
    }
    
    /**
     * 设置蜂鸣器（HTTP格式）
     * @param time 蜂鸣器持续时间（毫秒）
     * @return HTTP命令字符串
     */
    public String setBeepHttp(int time) {
        if (debug) {
            Log.d(TAG, "setBeepHttp: time=" + time);
        }
        
        return generateHttpCommand(HTTP_CMD_BEEP, "time=" + time);
    }
    
    /**
     * 设置机械臂舵机角度（HTTP格式）
     * @param servoId 舵机ID
     * @param angle 角度
     * @return HTTP命令字符串
     */
    public String setArmAngleHttp(int servoId, int angle) {
        if (debug) {
            Log.d(TAG, "setArmAngleHttp: servoId=" + servoId + ", angle=" + angle);
        }
        
        // 限制角度范围
        if (angle < 0) angle = 0;
        if (angle > 180) angle = 180;
        
        return generateHttpCommand(HTTP_CMD_ARM, "id=" + servoId + "&angle=" + angle);
    }

    /**
     * 将TCP命令转换为HTTP命令
     * @param tcpCommand TCP命令字符串
     * @return HTTP命令字符串，如果无法转换则返回null
     */
    public String convertTcpToHttp(String tcpCommand) {
        if (debug) {
            Log.d(TAG, "尝试将TCP命令转换为HTTP命令: " + tcpCommand);
        }
        
        if (tcpCommand == null || tcpCommand.length() < 9) {
            return null;
        }
        
        // 检查命令格式
        if (!tcpCommand.startsWith("$") || !tcpCommand.endsWith("#")) {
            return null;
        }
        
        try {
            // 提取功能码
            String funcCodeStr = tcpCommand.substring(3, 5);
            int funcCode = Integer.parseInt(funcCodeStr, 16);
            
            switch (funcCode) {
                case FUNC_CAR_RUN: // 小车运行
                    if (tcpCommand.length() >= 12) {
                        int state = Integer.parseInt(tcpCommand.substring(7, 9), 16);
                        int speed = Integer.parseInt(tcpCommand.substring(9, 11), 16);
                        
                        // 转换为HTTP命令
                        return setCarRunHttp(state, speed);
                    }
                    break;
                    
                case FUNC_SERVO: // 舵机控制
                    if (tcpCommand.length() >= 12) {
                        int servoId = Integer.parseInt(tcpCommand.substring(7, 9), 16);
                        int angle = Integer.parseInt(tcpCommand.substring(9, 11), 16);
                        
                        // 转换为HTTP命令
                        return setServoAngleHttp(servoId, angle);
                    }
                    break;
                    
                case FUNC_BEEP: // 蜂鸣器
                    if (tcpCommand.length() >= 12) {
                        int state = Integer.parseInt(tcpCommand.substring(7, 9), 16);
                        int delay = Integer.parseInt(tcpCommand.substring(9, 11), 16);
                        
                        int time = 0;
                        if (state > 0) {
                            if (delay == 255) {
                                time = 1;
                            } else {
                                time = delay * 10;
                            }
                        }
                        
                        // 转换为HTTP命令
                        return setBeepHttp(time);
                    }
                    break;
                    
                case FUNC_LED: // LED控制
                    if (tcpCommand.length() >= 14) {
                        int ledId = Integer.parseInt(tcpCommand.substring(7, 9), 16);
                        int r = Integer.parseInt(tcpCommand.substring(9, 11), 16);
                        int g = Integer.parseInt(tcpCommand.substring(11, 13), 16);
                        int b = Integer.parseInt(tcpCommand.substring(13, 15), 16);
                        
                        // 转换为HTTP命令
                        return setLedColorHttp(ledId, r, g, b);
                    }
                    break;
                    
                case FUNC_BTN_CONTROL: // 按键控制
                    if (tcpCommand.length() >= 10) {
                        int state = Integer.parseInt(tcpCommand.substring(7, 9), 16);
                        
                        // 转换为HTTP命令
                        return setCarRunHttp(state, 50); // 默认速度50
                    }
                    break;
            }
        } catch (Exception e) {
            Log.e(TAG, "转换TCP命令出错: " + e.toString());
        }
        
        return null;
    }
    
    /**
     * 将HTTP命令转换为TCP命令
     * @param httpCommand HTTP命令字符串
     * @return TCP命令字符串，如果无法转换则返回null
     */
    public String convertHttpToTcp(String httpCommand) {
        if (debug) {
            Log.d(TAG, "尝试将HTTP命令转换为TCP命令: " + httpCommand);
        }
        
        if (httpCommand == null || httpCommand.isEmpty()) {
            return null;
        }
        
        try {
            // 解析HTTP命令
            String[] parts = httpCommand.split("\\?");
            if (parts.length != 2) {
                return null;
            }
            
            String cmdType = parts[0];
            String params = parts[1];
            
            // 解析参数
            String[] paramPairs = params.split("&");
            
            if (HTTP_CMD_MOVE.equals(cmdType)) {
                // 移动命令
                String direction = null;
                int speed = 0;
                
                for (String pair : paramPairs) {
                    String[] keyValue = pair.split("=");
                    if (keyValue.length == 2) {
                        if ("direction".equals(keyValue[0])) {
                            direction = keyValue[1];
                        } else if ("speed".equals(keyValue[0])) {
                            try {
                                speed = Integer.parseInt(keyValue[1]);
                            } catch (NumberFormatException e) {
                                Log.e(TAG, "解析速度参数出错: " + e.toString());
                            }
                        }
                    }
                }
                
                if (direction != null) {
                    int state;
                    switch (direction) {
                        case "forward":
                            state = MOTOR_FORWARD;
                            break;
                        case "backward":
                            state = MOTOR_BACKWARD;
                            break;
                        case "left":
                            state = MOTOR_LEFT;
                            break;
                        case "right":
                            state = MOTOR_RIGHT;
                            break;
                        default:
                            state = MOTOR_STOP;
                            break;
                    }
                    
                    return setCarRun(state, speed);
                }
            } else if (HTTP_CMD_SERVO.equals(cmdType)) {
                // 舵机命令
                int servoId = 0;
                int angle = 0;
                
                for (String pair : paramPairs) {
                    String[] keyValue = pair.split("=");
                    if (keyValue.length == 2) {
                        if ("id".equals(keyValue[0])) {
                            try {
                                servoId = Integer.parseInt(keyValue[1]);
                            } catch (NumberFormatException e) {
                                Log.e(TAG, "解析舵机ID参数出错: " + e.toString());
                            }
                        } else if ("angle".equals(keyValue[0])) {
                            try {
                                angle = Integer.parseInt(keyValue[1]);
                            } catch (NumberFormatException e) {
                                Log.e(TAG, "解析角度参数出错: " + e.toString());
                            }
                        }
                    }
                }
                
                return setServoAngle(servoId, angle);
            }
        } catch (Exception e) {
            Log.e(TAG, "转换HTTP命令出错: " + e.toString());
        }
        
        return null;
    }
} 